#include "rpe_ltp_rx.h"

// RPE decoding clause(5.3.1)
void rpe_decoding(int Mc, int16_t xmaxcr,int16_t* xMcr,int16_t* erp) {
	
}
// Long term synthesis filtering(5.3.2)
void lt_synth_filter(int bcr,int Ncr,int16_t* drp,int16_t* drpp,int16_t* erp,int k) {
	static int nrp = 40;
	int Nr,brp;
	int i,k_start,k_end;

	k_start = k * 40;
	k_end = k_start + 39;

	static int16_t prev_drp[120]={0}; // drp[-1..-120]

	Nr = Ncr;
	if ( Ncr < 40 ) Nr = nrp;
	if ( Ncr > 120 ) Nr = nrp;
	nrp= Nr;
	brp = table_QLB[bcr];

	// Computation of the reconstructed short term residual signal drp[0..39].
	for(i=0;i<=39;i++) {
		drpp[k_start+i] = fxp_mult_r( brp, prev_drp[120+i-Nr] );// TODO fix the drp indexing
		drp[k_start+i] = fxp_add( erp[k_start+i], drpp[k_start+i] );
	}
	// Update of the reconstructed short term residual signal drp[-1..-120].
	// shift it to the left 8 positions
	for(i=0;i<80;i++)
		prev_drp[i] = prev_drp[40+i];
	// copy new samples
	for(i=0;i<20;i++)
		prev_drp[80+i]=drp[k_start+i];
//	for(i=0;i<=119;i++)
//		drp[-120+i] = drp[-80+i];
/*
This procedure uses the bcr and Ncr parameter to realize the long term synthesis filtering. The decoding of bcr needs the
use of table 5.3b.
- Nr is the received and decoded LTP lag.
- An array drp[-120..39] is used in this procedure.
The elements for -120 to -1 of the array drp are kept in memory for the long term synthesis filter. For each sub-segment
(40 samples), this procedure computes the drp[0..39] to be fed to the synthesis filter.
Check the limits of Nr.
Nr = Ncr;
IF ( Ncr < 40 ) THEN Nr = nrp;
IF ( Ncr > 120 ) THEN Nr = nrp;
nrp= Nr;
Keep the nrp value for the next sub-segment.
Initial value: nrp=40;
Decoding of the LTP gain bcr.
brp = QLB[bcr]
Computation of the reconstructed short term residual signal drp[0..39].
|== FOR k = 0 to 39:
| drpp = mult_r( brp, drp[k-Nr] );
| drp[k] = add( erp[k], drpp );
|== NEXT k:
Update of the reconstructed short term residual signal drp[-1..-120].
|== FOR k = 0 to 119:
| drp[-120+k] = drp[-80+k];
|== NEXT k:
Keep the array drp[-120..-1] for the next sub-segment.
Initial value: drp[-120..-1]=0;
 */
}
// Computation of the decoded reflection coefficients(5.3.3)
//void decode_lar(int16_t* LARcr, int16_t* rpr) {
//}

// Short term synthesis filtering clause(5.3.4)
//void st_synth_filter(int16_t* drp, int16_t* sr) {
//}

// De-emphasis filtering(5.3.5)
void de_emphasis_filter(int16_t* sr,int16_t* sro) {
	int16_t temp;
	int k;
	static int16_t msr=0; 
	for(k = 0 ; k<=159; k++) {
		temp = fxp_add( sr[k], fxp_mult_r( msr, 28180 ) );
		msr = temp;
		sro[k] = msr;
	}
/*
|== FOR k = 0 to 159:
| temp = add( sr[k], mult_r( msr, 28180 ) );
| msr = temp;
| sro[k] = msr;
|== NEXT k:
Keep msr in memory for the next frame.
Initial value: msr=0;
*/
}
// Upscaling of the output signal(5.3.6)
void upscaling_output(int16_t* sro, int16_t* srop) {
	int k;
	for(k=0;k<=159;k++)
		srop[k] = fxp_add( sro[k], sro[k] );
/*
|== FOR k = 0 to 159:
| srop[k] = add( sro[k], sro[k] );
|== NEXT k:
*/

}
// Truncation of the output variable(5.3.7)
void truncation_output(int16_t* srop) {
	int k;
	for(k=0;k<=159;k++) {
		srop[k] = srop[k] >> 3;
		srop[k] = srop[k] << 3;
	}
/*
|== FOR k = 0 to 159:
| srop[k] = srop[k] >> 3;
| srop[k] = srop[k] << 3;
|== NEXT k:
 */
}

void rpeltp_decoder(
		// For each primary frame
		int16_t* LARc,
		// For each secondary frame
		int16_t* LTP_P1,
		int16_t* LTP_b1,
		int16_t* RPE_M1,
		int16_t* RPE_xmaxc1,
		int16_t* RPE_xMp1,

		int16_t* LTP_P2,
		int16_t* LTP_b2,
		int16_t* RPE_M2,
		int16_t* RPE_xmaxc2,
		int16_t* RPE_xMp2,

		int16_t* LTP_P3,
		int16_t* LTP_b3,
		int16_t* RPE_M3,
		int16_t* RPE_xmaxc3,
		int16_t* RPE_xMp3,

		int16_t* LTP_P4,
		int16_t* LTP_b4,
		int16_t* RPE_M4,
		int16_t* RPE_xmaxc4,
		int16_t* RPE_xMp4,
		// Output
		// Primary voice frame
		int16_t* p_frame
		)
{
	// State variables
	static int16_t erp[160];
	static int16_t drp[160];
	static int16_t drpp[160];
//	static int16_t rrp[32];
	static int16_t sr[160];
	static int16_t sro[160];
	static int16_t xMp[13];
	// State variables for ST filter
	static int16_t LARpp[8];
	static int16_t prev_LARpp[8];
	static int16_t LARp[8];
	static int16_t rp[8];
	static int16_t u[8];

	// Other variables
	int16_t* LTP_P[] = {LTP_P1,LTP_P2,LTP_P3,LTP_P4};
	int16_t* LTP_b[] = {LTP_b1,LTP_b2,LTP_b3,LTP_b4};
	int16_t* RPE_M[] = {RPE_M1,RPE_M2,RPE_M3,RPE_M4};
	int16_t* RPE_xmaxc[] = {RPE_xmaxc1,RPE_xmaxc2,RPE_xmaxc3,RPE_xmaxc4};
	int16_t* RPE_xMc[] = {RPE_xMp1,RPE_xMp2,RPE_xMp3,RPE_xMp4};
	int k=0;
	int mant,exp;

	// TODO : Unpack homing frame
	// ?
	// Extract data from each secondary frame
	for(k=0;k<4;k++) {
		// RPE decoding clause(5.3.1)
		// rpe_decoding(RPE_M[k],RPE_xmaxc[k],RPE_xMc[k],erp,k);
		// APCM quantization of the selected RPE sequence(5.2.15)
		// apcm_q_of_rpe_seq(xM,RPE_xmaxc[k], &mant, &exp, RPE_xMc[k],k);
		exp=0;
		if ( *RPE_xmaxc[k] > 15 ) exp = fxp_sub( ( *RPE_xmaxc[k] >> 3 ), 1 ) ;
		mant = fxp_sub( *RPE_xmaxc[k] , ( exp << 3 ) );
		// APCM inverse quantization(5.2.16)
		apcm_iq_of_rpe_seq(RPE_xMc[k], mant, exp,xMp,k);
		// RPE grid positioning(5.2.17)
		rpe_grid_positioning(xMp, *RPE_M[k],erp,k);		
		// Long term synthesis filtering(5.3.2)
		lt_synth_filter(*LTP_b[k],*LTP_P[k],drp,drpp,erp,k);
	}

	// Computation of the decoded reflection coefficients(5.3.3)
	// decode_lar(LARc,rrp);
	larc_to_larpp(LARc,LARpp);
	// Short term synthesis filtering clause(5.3.4)
	// st_synth_filter(drp, sr);
	st_filter(drp,sr,LARpp,prev_LARpp,u,rp,LARp);
	// De-emphasis filtering(5.3.5)
	de_emphasis_filter(sr,sro);
	// Upscaling of the output signal(5.3.6)
	upscaling_output(sro, p_frame);
	// Truncation of the output variable(5.3.7)
	truncation_output(p_frame);	

}

void unpack_decoder_homing_frame(
				// Input
				// For each primary frame
				int16_t* LARc,
				// For each secondary frame
				int16_t* LTP_P1,
				int16_t* LTP_b1,
				int16_t* RPE_M1,
				int16_t* RPE_xmaxc1,
				int16_t* RPE_xMc1,

				int16_t* LTP_P2,
				int16_t* LTP_b2,
				int16_t* RPE_M2,
				int16_t* RPE_xmaxc2,
				int16_t* RPE_xMc2,

				int16_t* LTP_P3,
				int16_t* LTP_b3,
				int16_t* RPE_M3,
				int16_t* RPE_xmaxc3,
				int16_t* RPE_xMc3,

				int16_t* LTP_P4,
				int16_t* LTP_b4,
				int16_t* RPE_M4,
				int16_t* RPE_xmaxc4,
				int16_t* RPE_xMc4,
				// Output - Packed voice frame
				uint8_t* dhf
		) {

	// Pack the LAR coefficients
	// TODO : check the shifts
	// byte 0  : 6 msb LARc[0], 2 lsb LARc[1]
	LARc[0]       = get_bit_range(dhf[0],7,2);
	LARc[1]       = get_bit_range(dhf[0],2,0)<<4;
	// byte 1  : 4 msb LARc[1], 4 lsb LARc[2]
	LARc[1]      |= get_bit_range(dhf[1],7,4);
	LARc[2]       = get_bit_range(dhf[1],3,0)<<1;
	// byte 2  : 1 msb LARc[2], 5 lsb LARc[3] , 2 lsb LARc[4]
	LARc[2]      |= get_bit_range(dhf[2],7,7);// TODO : check to see if macro supports this case
	LARc[3]       = get_bit_range(dhf[2],6,2);
	LARc[4]       = get_bit_range(dhf[2],1,0)<<2;
	// byte 3  : 2 msb LARc[4], 4 lsb LARc[5], 2 lsb LARc[6]
	LARc[4]      |= get_bit_range(dhf[3],7,6);
	LARc[5]       = get_bit_range(dhf[3],5,2);
	LARc[6]       = get_bit_range(dhf[3],1,0)<<1;
	// byte 4  : 1 msb LARc[6], 3 lsb LARc[7], 4 lsb N1
	LARc[6]      |= get_bit_range(dhf[4],7,7);
	LARc[7]       = get_bit_range(dhf[4],6,4);
	*LTP_P1       = get_bit_range(dhf[4],3,0)<<3;

	// byte 5  : 3 msb N1, 2 lsb b1, 2 lsb M1, 1 lsb Xmax1
	*LTP_P1      |= get_bit_range(dhf[5],7,5);
	*LTP_b1       = get_bit_range(dhf[5],4,3);
	*RPE_M1       = get_bit_range(dhf[5],2,1);
	*RPE_xmaxc1   = get_bit_range(dhf[5],0,0)<<5;
	// byte 6  : 5 msb Xmax1, 3 lsb x1(0)
	*RPE_xmaxc1  |= get_bit_range(dhf[6],7,3);
	RPE_xMc1[0]   = get_bit_range(dhf[6],2,0);
	// byte 7  : 3 msb x1(1), 3 lsb x1(2), 2 lsb x1(3)
	RPE_xMc1[1]   = get_bit_range(dhf[7],7,5);
	RPE_xMc1[2]   = get_bit_range(dhf[7],4,2);
	RPE_xMc1[3]   = get_bit_range(dhf[7],1,0)<<1;
	// byte 8  : 1 msb x1(3), 3 lsb x1(4), 3 lsb x1(5), 1 lsb x1(6)
	RPE_xMc1[3]  |= get_bit_range(dhf[8],7,7);
	RPE_xMc1[4]   = get_bit_range(dhf[8],6,4);
	RPE_xMc1[5]   = get_bit_range(dhf[8],3,1);
	RPE_xMc1[6]   = get_bit_range(dhf[8],0,0)<<2;
	// byte 9  : 2 lsb x1(6), 3 lsb x1(7), 3 lsb x1(8)
	RPE_xMc1[6]  |= get_bit_range(dhf[9],7,6);
	RPE_xMc1[7]   = get_bit_range(dhf[9],5,3);
	RPE_xMc1[8]   = get_bit_range(dhf[9],2,0);
	// byte 10 : 3 lsb x1(9), 3 lsb x1(10), 2 lsb x1(11)
	RPE_xMc1[9]   = get_bit_range(dhf[10],7,5);
	RPE_xMc1[10]  = get_bit_range(dhf[10],4,2);
	RPE_xMc1[11]  = get_bit_range(dhf[10],1,0)<<1;
	// byte 11 : 1 lsb x1(11), 3 lsb x1(12), 4 lsb N2
	RPE_xMc1[11] |= get_bit_range(dhf[11],7,7);
	RPE_xMc1[12]  = get_bit_range(dhf[11],6,4);
	*LTP_P2       = get_bit_range(dhf[11],3,0)<<0;

	// byte 12 : 3 msb N2, 2 lsb b2, 2 lsb M2, 1 lsb Xmax2
	*LTP_P2      |= get_bit_range(dhf[12],7,5);
	*LTP_b2       = get_bit_range(dhf[12],4,3);
	*RPE_M2       = get_bit_range(dhf[12],2,1);
	*RPE_xmaxc2   = get_bit_range(dhf[12],0,0)<<5;
	// byte 13 : 5 msb Xmax1, 3 lsb x2(0)
	*RPE_xmaxc2  |= get_bit_range(dhf[13],7,3);
	RPE_xMc2[0]   = get_bit_range(dhf[13],2,0);
	// byte 14 : 3 msb x2(1), 3 lsb x2(2), 2 lsb x2(3)
	RPE_xMc2[1]   = get_bit_range(dhf[14],7,5);
	RPE_xMc2[2]   = get_bit_range(dhf[14],4,2);
	RPE_xMc2[3]   = get_bit_range(dhf[14],1,0)<<1;
	// byte 15 : 1 msb x2(3), 3 lsb x2(4), 3 lsb x2(5), 1 lsb x2(6)
	RPE_xMc2[3]  |= get_bit_range(dhf[15],7,7);
	RPE_xMc2[4]   = get_bit_range(dhf[15],6,4);
	RPE_xMc2[5]   = get_bit_range(dhf[15],3,1);
	RPE_xMc2[6]   = get_bit_range(dhf[15],0,0)<<2;
	// byte 16 : 2 lsb x2(6), 3 lsb x2(7), 3 lsb x2(8)
	RPE_xMc2[6]  |= get_bit_range(dhf[16],7,6);
	RPE_xMc2[7]   = get_bit_range(dhf[16],5,3);
	RPE_xMc2[8]   = get_bit_range(dhf[16],2,0);
	// byte 17 : 3 lsb x2(9), 3 lsb x2(10), 2 lsb x2(11)
	RPE_xMc2[9]   = get_bit_range(dhf[17],7,5);
	RPE_xMc2[10]  = get_bit_range(dhf[17],4,2);
	RPE_xMc2[11]  = get_bit_range(dhf[17],1,0)<<1;
	// byte 18 : 1 lsb x2(11), 3 lsb x2(12), 4 lsb N2
	RPE_xMc2[11] |= get_bit_range(dhf[18],7,7);
	RPE_xMc2[12]  = get_bit_range(dhf[18],6,4);
	*LTP_P2       = get_bit_range(dhf[18],3,0)<<0;

	// byte 19 : 3 msb N3, 2 lsb b3, 2 lsb M3, 1 lsb Xmax3
	*LTP_P3      |= get_bit_range(dhf[19],7,5);
	*LTP_b3       = get_bit_range(dhf[19],4,3);
	*RPE_M3       = get_bit_range(dhf[19],2,1);
	*RPE_xmaxc3   = get_bit_range(dhf[19],0,0)<<5;
	// byte 20 : 5 msb Xmax3, 3 lsb x3(0)
	*RPE_xmaxc3  |= get_bit_range(dhf[20],7,3);
	RPE_xMc3[0]   = get_bit_range(dhf[20],2,0);
	// byte 21 : 3 msb x3(1), 3 lsb x3(2), 2 lsb x3(3)
	RPE_xMc3[1]   = get_bit_range(dhf[21],7,5);
	RPE_xMc3[2]   = get_bit_range(dhf[21],4,2);
	RPE_xMc3[3]   = get_bit_range(dhf[21],1,0)<<1;
	// byte 22 : 1 msb x3(3), 3 lsb x3(4), 3 lsb x3(5), 1 lsb x3(6)
	RPE_xMc3[3]  |= get_bit_range(dhf[22],7,7);
	RPE_xMc3[4]   = get_bit_range(dhf[22],6,4);
	RPE_xMc3[5]   = get_bit_range(dhf[22],3,1);
	RPE_xMc3[6]   = get_bit_range(dhf[22],0,0)<<2;
	// byte 23 : 2 lsb x3(6), 3 lsb x3(7), 3 lsb x3(8)
	RPE_xMc3[6]  |= get_bit_range(dhf[23],7,6);
	RPE_xMc3[7]   = get_bit_range(dhf[23],5,3);
	RPE_xMc3[8]   = get_bit_range(dhf[23],2,0);
	// byte 24 : 3 lsb x3(9), 3 lsb x3(10), 2 lsb x3(11)
	RPE_xMc3[9]   = get_bit_range(dhf[24],7,5);
	RPE_xMc3[10]  = get_bit_range(dhf[24],4,2);
	RPE_xMc3[11]  = get_bit_range(dhf[24],1,0)<<1;
	// byte 25 : 1 lsb x3(11), 3 lsb x3(12), 4 lsb N2
	RPE_xMc3[11] |= get_bit_range(dhf[25],7,7);
	RPE_xMc3[12]  = get_bit_range(dhf[25],6,4);
	*LTP_P2       = get_bit_range(dhf[25],3,0)<<0;

	// byte 26 : 3 msb N4, 2 lsb b4, 2 lsb M4, 1 lsb Xmax4
	*LTP_P4      |= get_bit_range(dhf[26],7,5);
	*LTP_b4       = get_bit_range(dhf[26],4,3);
	*RPE_M4       = get_bit_range(dhf[26],2,1);
	*RPE_xmaxc4   = get_bit_range(dhf[26],0,0)<<5;
	// byte 27 : 5 msb Xmax4, 3 lsb x4(0)
	*RPE_xmaxc4  |= get_bit_range(dhf[27],7,3);
	RPE_xMc4[0]   = get_bit_range(dhf[27],2,0);
	// byte 28 : 3 msb x4(1), 3 lsb x4(2), 2 lsb x4(3)
	RPE_xMc4[1]   = get_bit_range(dhf[28],7,5);
	RPE_xMc4[2]   = get_bit_range(dhf[28],4,2);
	RPE_xMc4[3]   = get_bit_range(dhf[28],1,0)<<1;
	// byte 29 : 1 msb x4(3), 3 lsb x4(4), 3 lsb x4(5), 1 lsb x4(6)
	RPE_xMc4[3]  |= get_bit_range(dhf[29],7,7);
	RPE_xMc4[4]   = get_bit_range(dhf[29],6,4);
	RPE_xMc4[5]   = get_bit_range(dhf[29],3,1);
	RPE_xMc4[6]   = get_bit_range(dhf[29],0,0)<<2;
	// byte 30 : 2 lsb x4(6), 3 lsb x4(7), 3 lsb x4(8)
	RPE_xMc4[6]  |= get_bit_range(dhf[30],7,6);
	RPE_xMc4[7]   = get_bit_range(dhf[30],5,3);
	RPE_xMc4[8]   = get_bit_range(dhf[30],2,0);
	// byte 31 : 3 lsb x4(9), 3 lsb x4(10), 2 lsb x4(11)
	RPE_xMc4[9]   = get_bit_range(dhf[31],7,5);
	RPE_xMc4[10]  = get_bit_range(dhf[31],4,2);
	RPE_xMc4[11]  = get_bit_range(dhf[31],1,0)<<1;
	// byte 32 : 1 lsb x4(11), 3 lsb x4(12)
	RPE_xMc4[11] |= get_bit_range(dhf[32],7,7);
	RPE_xMc4[12]  = get_bit_range(dhf[32],6,4);

}

